#include "capture.h"

#define CAMERA_BUF_NUM  1

static void save_jpeg(char *path, unsigned char *buf, ssize_t len) {
    int fd = open(path, O_RDWR | O_CREAT | O_TRUNC, 0664);    // "-rw-rw-r--"
    if (fd < 0) {
        LOG("open %s fail\n", path);
        return;
    }

    ssize_t ret = write(fd, buf, len);
    if (len != ret) {
        LOG("write %s fail\n", path);
    }

    close(fd);
}

static void device_set_fmt(struct camera_type *camera) {
    if (camera->state == CAMERA_STATE_OPEN) {
        struct v4l2_format format;

        CLEAR(format);
        format.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        format.fmt.pix.width = camera->width;
        format.fmt.pix.height = camera->height;
        format.fmt.pix.pixelformat = V4L2_PIX_FMT_MJPEG;
        format.fmt.pix.field = V4L2_FIELD_INTERLACED;
        int ret = ioctl(camera->fd, VIDIOC_S_FMT, &format);
        if (ret < 0) {
            LOG("set video format fail\n");
        }
    }
}

static void device_get_fmt(struct camera_type *camera) {
    struct v4l2_format format;

    CLEAR(format);
    format.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    int ret = ioctl(camera->fd, VIDIOC_G_FMT, &format);
    if (ret < 0) {
        LOG("get video format fail\n");
        return;
    }

    if (format.fmt.pix.width == camera->width && format.fmt.pix.height == camera->height) {
        LOG("set camera width and height success\n");
    }else{
        camera->width = format.fmt.pix.width;
        camera->height  = format.fmt.pix.height;
        LOG("use default camera width and height\n");
    }
    LOG("Camera width * height = %d * %d\n", camera->width, camera->height);
}

static void device_req_buf(struct camera_type *camera) {
    if (camera->state != CAMERA_STATE_OPEN) {
        return;
    }

    struct v4l2_requestbuffers req;
    CLEAR(req);
    req.count = CAMERA_BUF_NUM;
    req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    req.memory = V4L2_MEMORY_MMAP;

    int ret = ioctl(camera->fd, VIDIOC_REQBUFS, &req);
    if ( ret < 0) {
        LOG("request buffer fail\n");
        return;
    }

    camera->n_buffers = req.count;
    camera->state = CAMERA_STATE_REQ_BUF;
    LOG("Request buffer success\n");
}

static void device_map_buf(struct camera_type *camera) {
    if (camera->state != CAMERA_STATE_REQ_BUF) {
        return;
    }

    camera->buffers = calloc(camera->n_buffers, sizeof(*camera->buffers));
    int i;
    for (i = 0; i < camera->n_buffers; i++) {
        int ret;
        struct v4l2_buffer buf;
        CLEAR(buf);
        buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        buf.memory = V4L2_MEMORY_MMAP;
        buf.index = i;

        ret = ioctl(camera->fd, VIDIOC_QUERYBUF, &buf);
        if ( ret < 0) {
            LOG("query buffer fail\n");
            return;
        }

        camera->buffers[i].length = buf.length;
        camera->buffers[i].start = mmap(NULL, buf.length,
                                        PROT_READ | PROT_WRITE,MAP_SHARED,
                                        camera->fd, (long) buf.m.offset);

        if (MAP_FAILED == camera->buffers[i].start) {
            LOG("mmap buffers[%d] fail\n", i);
        }

        CLEAR(buf);
        buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        buf.memory = V4L2_MEMORY_MMAP;
        buf.index = i;

        ret = ioctl(camera->fd, VIDIOC_QBUF, &buf);
        if (ret < 0) {
            LOG("enqueue buffer fail\n");
        }
    }
    camera->state = CAMERA_STATE_MAP_BUF;
    LOG("Mapping space success\n");
}

static void device_munmap_buf(struct camera_type *camera) {
    if( camera->state != CAMERA_STATE_MAP_BUF){
        return;
    }
    int i;
    for (i = 0; i < camera->n_buffers; i++) {
        int ret = munmap(camera->buffers[i].start, camera->buffers[i].length);
        if (-1 == ret) {
            LOG("munmap fail\n");
        }
    }

    free(camera->buffers);
    camera->state = CAMERA_STATE_REQ_BUF;
    LOG("munmap space success\n");
}

static void device_stream_on(struct camera_type *camera) {
    if (camera->state != CAMERA_STATE_MAP_BUF) {
        return;
    }
    enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;

    if (ioctl(camera->fd, VIDIOC_STREAMON, &type) < 0) {
        perror("start Camera fail\n");
    }
    camera->state = CAMERA_STATE_STREAM_ON;
    LOG("camera is streaming\n");
}

static void device_stream_off(struct camera_type *camera) {
    if (camera->state != CAMERA_STATE_STREAM_ON) {
        return;
    }
    enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;

    int ret = ioctl(camera->fd, VIDIOC_STREAMOFF, &type);
    if (ret < 0) {
        LOG("stop Camera fail\n");
        return;
    }
    camera->state = CAMERA_STATE_MAP_BUF;
    LOG("camera is stop\n");
}

int camera_init(struct camera_type *camera) {
    if (camera->state == CAMERA_STATE_STREAM_ON) {
        LOG("\nCamera already init %s\n", camera->name);
        return 0;
    }

    if (camera->state == CAMERA_STATE_CLOSE) {
        camera->fd = open(camera->name, O_RDWR);
        if (camera->fd < 0) {
            LOG("open %s fail\n", camera->name);
            return -1;
        }

        camera->state = CAMERA_STATE_OPEN;
        LOG("Open %s, fd %d\n", camera->name, camera->fd);
    }

    device_set_fmt(camera);
    device_get_fmt(camera);
    device_req_buf(camera);
    device_map_buf(camera);
    device_stream_on(camera);

    if (camera->state == CAMERA_STATE_STREAM_ON) {
        LOG("init %s success\n\n", camera->name);
        return 0;
    } else {
        LOG("init %s fail\n\n", camera->name);
        return -1;
    }
}

int camera_capture(struct camera_type *camera, char *save_path) {

    if (camera->state != CAMERA_STATE_STREAM_ON) {
        LOG("Init Camera %s not completed\n", camera->name);
        return -1;
    }

    LOG("Start capture a frame\n");
    struct v4l2_buffer buf;
    CLEAR(buf);
    buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    buf.memory = V4L2_MEMORY_MMAP;

    int ret = ioctl(camera->fd, VIDIOC_DQBUF, &buf);
    if (ret < 0 || buf.index >= camera->n_buffers) {
        LOG("dequeue fail\n");
        return -4;
    }

    ret = ioctl(camera->fd, VIDIOC_QBUF, &buf);
    if (ret < 0) {
        LOG("enqueue fail\n");
        return -5;
    }

    ret = ioctl(camera->fd, VIDIOC_DQBUF, &buf);
    if (ret < 0 || buf.index >= camera->n_buffers) {
        LOG("dequeue fail\n");
        return -4;
    }

    save_jpeg(save_path, camera->buffers[buf.index].start, buf.bytesused);

    ret = ioctl(camera->fd, VIDIOC_QBUF, &buf);
    if (ret < 0) {
        LOG("enqueue fail\n");
        return -5;
    }

    LOG("Success capture a frame\n\n");

    return ret;
}

int camera_close(struct camera_type *camera) {
    device_stream_off(camera);
    device_munmap_buf(camera);

    if (camera->state > CAMERA_STATE_CLOSE) {
        close(camera->fd);
        LOG("Video [fd:%d] is delete\n", camera->fd);
        camera->fd = -1;
        camera->state = CAMERA_STATE_CLOSE;
    }
    LOG("Close camera success\n\n");
    return 0;
}